/** \file LiftManager.h
* \brief Useful functions for controlling the height of the lift.
*
* LiftManager.h contains the functions used in regulating the position of the lift, including reading the lift sensors and setting motor speeds.
*/

#pragma SystemFile

#include "PIDControlledMotor.h"
#include "drivers/hitechnic-protoboard.h"

///Contains a PID controller for the lift motors
PIDControlledMotor lift;

///Allows the runPIDController method to run concurrently with the main task of the program.
task runControllers()
{
	runPIDController(lift);
}

/**
 * \brief Manges the lift instance of the PID controller.
 *
 * Configures the lift PID controller and updates the sensor input and the motor power based upon the results of the PID controller.
 */
task moveLift()
{
	lift.Kp=.6;
	lift.Ki=.0002;
	lift.Kd=7;
	lift.Tp=0;
	lift.currentValue=HTPBreadADC(ProtoBoard,0,10);
		motor[LeftLift]=-100;
		motor[RightLift]=-100;
		motor[TopLift]=-100;
		wait10Msec(200);
	while((lift.currentValue)<(lift.target))
	{
		lift.currentValue=HTPBreadADC(ProtoBoard,0,10);
		motor[LeftLift]=-100;
		motor[RightLift]=-100;
		motor[TopLift]=-100;
		wait10Msec(10);
	  motor[LeftLift]=0;
		motor[RightLift]=0;
		motor[TopLift]=0;
		wait10Msec(15);
	}
 // while(HTPBreadADC(ProtoBoard,0,10)>(lift.target))
	//{
	//	motor[LeftLift]=100;
	//	motor[RightLift]=100;
	//	motor[TopLift]=100;
	//}
	motor[LeftLift]=0;
	motor[RightLift]=0;
	motor[TopLift]=0;
	//StartTask(runControllers);
	//wait10Msec(50);
	//while(true)
	//{
	//	lift.currentValue=HTPBreadADC(ProtoBoard,0,10);
	//	if(abs(lift.power)>=10)
	//	{
	//		motor[LeftLift]=lift.power;
	//		motor[RightLift]=lift.power;
	//		motor[TopLift]=lift.power;
	//	}
	//	else
	//	{
	//		motor[LeftLift]=0;
	//		motor[RightLift]=0;
	//		motor[TopLift]=0;
	//	}
	//	if(abs(lift.currentValue-lift.target)<=5)
	//	{
	//		motor[LeftLift]=0;
	//		motor[RightLift]=0;
	//		motor[TopLift]=0;
	//		return;
	//	}
	//}
}

/**
 * \brief Sets the lift to move to a specific position
 *
 * @param position The sensor value the lift should target and hold.
 */
void goToPosition(int position)
{
	StopTask(moveLift);
	lift.target=position;
	StartTask(moveLift);
}
